#ifndef _TELLOTOF_H
#define _TELLOTOF_H

#include <RMTT_Libs.h>

int get_3tof(uint8_t _num)
{
    unsigned char CMD_0[8] = {0x01, 0x03, 0x00, 0x34, 0x00, 0x02, 0x85, 0xc5};
    unsigned char CMD_1[8] = {0x02, 0x03, 0x00, 0x34, 0x00, 0x02, 0xc85, 0xF6};
    unsigned char CMD_2[8] = {0x03, 0x03, 0x00, 0x34, 0x00, 0x02, 0xc84, 0x27};
	unsigned char CMD_3[8] = {0x04, 0x03, 0x00, 0x34, 0x00, 0x02, 0xc85, 0x90};
    int nSensor1, nSensor2, nSensor3, nSensor4;
    int nRead;
    char rx_data[16];
    nSensor1 = 0;
    nSensor2 = 0;
    nSensor3 = 0;
	nSensor4 = 0;
    switch (_num)
    {
    case 1:
        Serial2.write(CMD_0, 8);
        delay(3);
        while (Serial2.available() > 0)
        {
            nRead = Serial2.available();
            Serial2.readBytes(rx_data, nRead);
			  if(rx_data[6]==0||rx_data[6]==7)
				nSensor1=rx_data[3]<<8 | rx_data[4];
			  else
			    nSensor1=9999;
            return nSensor1;
        }
        break;
    case 2:
        Serial2.write(CMD_1, 8);
        delay(3);
        while (Serial2.available() > 0)
        {
            nRead = Serial2.available();
            Serial2.readBytes(rx_data, nRead);
			if(rx_data[6]==0||rx_data[6]==7)
			  nSensor2=rx_data[3]<<8 | rx_data[4];
			else
			  nSensor2=9999;
            return nSensor2;
        }
        break;
    case 3:
        Serial2.write(CMD_2, 8);
        delay(3);
        while (Serial2.available() > 0)
        {
            nRead = Serial2.available();
            Serial2.readBytes(rx_data, nRead);
			if(rx_data[6]==0||rx_data[6]==7)
			  nSensor3=rx_data[3]<<8 | rx_data[4];
			else
			  nSensor3=9999;
            return nSensor3;
        }
        break;
	case 4:
		    Serial2.write(CMD_3, 8);
		    delay(3);
		    while (Serial2.available() > 0)
			
		    {
		        nRead = Serial2.available();
		        Serial2.readBytes(rx_data, nRead);
				if(rx_data[6]==0||rx_data[6]==7)
				  nSensor4=rx_data[3]<<8 | rx_data[4];
				else
				  nSensor4=9999;
		        return nSensor4;
		    }
		    break;
    default:
        return 0;
        break;
    }
}

#endif